#ifndef DATA_HOLDER_H_
#define DATA_HOLDER_H_

#include <string.h>
#include <stdint.h>

#pragma pack(push)
#pragma pack(1)

typedef int int32;
typedef short int16;
typedef unsigned short uint16;

struct Robot_firmware{
    char version[16]; //固件版本
    char time[16];  //构建时间
};

struct Robot_parameter{
    union{
        char buff[64];
        struct{   
            unsigned short wheel_diameter;   //轮子直径  mm
            unsigned short wheel_track;      //差分：轮距， 三全向轮：直径，四全向：前后轮距+左右轮距 mm
            unsigned short encoder_resolution;  //编码器分辨率
            unsigned char do_pid_interval;         //pid间隔 (ms)
            unsigned short kp;
            unsigned short ki;
            unsigned short kd;
            unsigned short ko;                  //pid参数比例
            unsigned short cmd_last_time;       //命令持久时间ms 超过该时间会自动停止运动
            unsigned short max_v_liner_x;
            unsigned short max_v_liner_y;
            unsigned short max_v_angular_z;
            unsigned char imu_type;
        };
    };
};

struct Robot_velocity{
    short v_liner_x; //线速度 前>0 mm/s
    short v_liner_y; //差分轮 为0  mm/s
    short v_angular_z; //角速度 左>0 0.001rad/s  1000 means 1 rad/s
};

struct Robot_odom{
    short v_liner_x;      //线速度 前>0 后<0  mm/s
    short v_liner_y;      //差分轮 为0        mm/s
    short v_angular_z;    //角速度 左>0 右<0  0.001rad/s   1000 means 1 rad/s
    int32 x;              //里程计坐标x       mm (这里long为4字节，下同)
    int32 y;              //里程计坐标y       mm
    short yaw;            //里程计航角        0.01rad     100 means 1 rad
};

struct Robot_pid_data{
    int32 input[4];  //各轮子驱动输入值
    int32 output[4]; //个轮子输出值
};

typedef struct _SetHydrauArm{
    struct{
        unsigned char stretch;
        unsigned char drawback;
        unsigned char pump;             //0x00 Off, Other On
    }arms[2];                           //Left or Right
}SetHydrauArm_T;

typedef struct _SetHydroRaise{
    uint8_t up;
    uint8_t down;
    uint8_t pump;
}SetHydroRaise_T;

struct Robot_each_motor{
    union{
        short speed[8];
        struct{
            short vL;   //左电机
            short vR;   //右电机
        }wheels[4];
    }un; 
};

typedef struct {
    short angular_vel;
    short wheel_vel;
}SetSpin_T;

typedef struct {
    unsigned char enable;
    unsigned char position;
    unsigned int distance;
}SetDoubleCar_T;

typedef struct _RobotState{
    struct{
        float angles[4];                //四个驱动的旋转角度
        float height[4];                //四个驱动的高度
        unsigned short smallGear;       //小齿轮齿数23
        unsigned short bigGear;         //大齿轮齿数141
    }wheels;
    SetDoubleCar_T team_info;            //是否双车组队
}RobotState_T;

/* 设置odom的偏移量，修正odom偏差 */
typedef struct _OdomTf{
    float dx;
    float dy;
    float dth;
}OdomTf_T;

#pragma pack(pop)

typedef struct _MsgFlag{
    bool version;
    bool odom;
    bool robot_state;
}MsgFlag_T;

class Data_holder{
    public:
        static Data_holder* get(){
            static Data_holder dh;
            return &dh;
        }

        void load_parameter();

        void save_parameter();
    
    private:
        Data_holder(){
            memset(&firmware_info, 0, sizeof(struct Robot_firmware));
            memset(&parameter, 0, sizeof(struct Robot_parameter));
            memset(&velocity, 0, sizeof(struct Robot_velocity));
            memset(&odom, 0, sizeof(struct Robot_odom));
            memset(&pid_data, 0, sizeof(struct Robot_pid_data));
            memset(&imu_data, 0, sizeof(imu_data));
            memset(&each_motor, 0, sizeof(each_motor));
            memset(&hydraulic_raise, 0, sizeof(hydraulic_raise));
            memset(&hydraulic_arm, 0, sizeof(hydraulic_arm));
            memset(&robot_spin, 0, sizeof(robot_spin));
            memset(&double_car, 0, sizeof(double_car));
            memset(&robot_state, 0, sizeof(robot_state));
            memset(&odom_tf, 0, sizeof(odom_tf));
            }

    
    public:
        MsgFlag_T msg_flags;
            
    public:
        struct Robot_firmware  firmware_info;
        struct Robot_parameter  parameter;
        struct Robot_velocity  velocity;
        struct Robot_odom      odom;
        struct Robot_pid_data  pid_data;
        struct Robot_each_motor each_motor;
        SetHydroRaise_T hydraulic_raise;
        SetHydrauArm_T hydraulic_arm;
        SetSpin_T robot_spin;
        SetDoubleCar_T double_car;
        RobotState_T robot_state;
        OdomTf_T odom_tf;

        float imu_data[9];
};
#endif